#include "../include/Matcher.hpp"
namespace ORBSLAM
{
    Matcher::Matcher(float ratio, bool check_ori, int th_low, int th_high, int bin_len) : mf_best_secondbest_ratio(ratio), mb_check_orientation(check_ori), mi_bin_len(bin_len), mi_th_high(th_high), mi_th_low(th_low)
    {
    }

    void Matcher::Set_low_dist_th(int th)
    {
        mi_th_low = th;
    }

    void Matcher::Set_ratio(float ratio)
    {
        mf_best_secondbest_ratio = ratio;
    }

    void Matcher::Set_if_check_orientation(bool b)
    {
        mb_check_orientation = b;
    }

    void Matcher::Set_bin_len(int bin_len)
    {
        mi_bin_len = bin_len;
    }

    int Matcher::Search_for_initialization(Frame &F1, Frame &F2, vector<Point2f> &vp_prematched, vector<int> &vn_matches12, int window_size, vector<pair<int, int>> &match_score)
    {
        int nmatches = 0;
        const float bin_factor = mi_bin_len / 360.0f;
        vector<vector<int>> rot_hist(mi_bin_len);
        vn_matches12 = vector<int>(F1.mn_feature_num, -1);
        match_score = vector<pair<int, int>>(F1.mn_feature_num, make_pair(256, -1));
        vector<int> vn_matches21(F2.mn_feature_num, -1);
        vector<int> v_matched_descriptors_distance(F2.mn_feature_num, INT_MAX);

        for (int i = 0; i < mi_bin_len; i++)
        {
            rot_hist[i].reserve(500);
        }
        int F1_kp_size = F1.mv_orb_unkeypoints.size();
        // TMP
        // cout << "descriptors:" << F1.mm_descriptors.row(3392) << endl;

        for (int i1 = 0; i1 < F1_kp_size; i1++)
        {
            KeyPoint kp1 = F1.mv_orb_unkeypoints[i1];
            int level1 = kp1.octave;
            // TODO why
            if (level1 > 0)
            {
                continue;
            }

            vector<int> candidate_index_inF2 = F2.Get_candidate_points_to_match(vp_prematched[i1], window_size, level1, level1);
            if (candidate_index_inF2.empty())
            {
                continue;
            }
            Mat descriptors1 = F1.mm_descriptors.row(i1);
            int best_dist = INT_MAX;
            int second_best_dist = INT_MAX;
            int best_idx = -1;
            for (vector<int>::iterator it = candidate_index_inF2.begin(); it != candidate_index_inF2.end(); it++)
            {
                int i2 = *it;
                Mat descriptors2 = F2.mm_descriptors.row(i2);
                int dist = Compute_descriptor_distance(descriptors1, descriptors2);
                if (v_matched_descriptors_distance[i2] <= dist)
                {
                    continue;
                }
                if (dist < best_dist)
                {
                    second_best_dist = best_dist;
                    best_dist = dist;
                    best_idx = i2;
                }
                else if (dist < second_best_dist)
                {
                    second_best_dist = dist;
                }
            }
            if (best_dist <= mi_th_low)
            {
                if (best_dist < (float)second_best_dist * mf_best_secondbest_ratio)
                {
                    if (vn_matches21[best_idx] >= 0)
                    {
                        vn_matches12[vn_matches21[best_idx]] = -1;
                        match_score[vn_matches21[best_idx]] = make_pair(-1, -1);
                        nmatches--;
                    }
                    vn_matches12[i1] = best_idx;
                    match_score[i1] = make_pair(best_dist, best_idx);
                    vn_matches21[best_idx] = i1;
                    v_matched_descriptors_distance[best_idx] = best_dist;
                    nmatches++;
                    if (mb_check_orientation)
                    {
                        float rot = F1.mv_orb_unkeypoints[i1].angle - F2.mv_orb_unkeypoints[best_idx].angle;
                        if (rot < 0.0)
                        {
                            rot += 360.0f;
                        }
                        int bin = cvFloor(rot * bin_factor);
                        if (bin == mi_bin_len)
                        {
                            bin = 0;
                        }
                        assert(bin >= 0 && bin < mi_bin_len);
                        rot_hist[bin].push_back(i1);
                    }
                }
            }
        }
        if (mb_check_orientation)
        {
            int idx1 = -1;
            int idx2 = -1;
            int idx3 = -1;
            Compute_three_maxima(rot_hist, mi_bin_len, idx1, idx2, idx3);

            for (int i = 0; i < mi_bin_len; i++)
            {
                if (i == idx1 || i == idx2 || i == idx3)
                {
                    continue;
                }
                for (int j = 0; j < rot_hist[i].size(); j++)
                {
                    int idx = rot_hist[i][j];
                    if (vn_matches12[idx] >= 0)
                    {
                        vn_matches12[idx] = -1;
                        nmatches--;
                    }
                }
            }
        }
        for (int i = 0; i < vn_matches12.size(); i++)
        {
            if (vn_matches12[i] >= 0)
            {
                vp_prematched[i] = F2.mv_orb_unkeypoints[vn_matches12[i]].pt;
            }
        }
        return nmatches;
    }

    int Matcher::Compute_descriptor_distance(const Mat &descriptor1, const Mat &descriptor2)
    {
        const int *p1 = descriptor1.ptr<int32_t>();
        const int *p2 = descriptor2.ptr<int32_t>();
        int dist = 0;
        for (int i = 0; i < 8; i++, p1++, p2++)
        {
            unsigned int v = *p1 ^ *p2;
            v = v - ((v >> 1) & 0x55555555);
            v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
            dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
        }
        return dist;
    }

    int Matcher::Search_by_bow(KeyFrame *p_refKF, Frame &cur_frame, vector<MapPoint *> &vp_mpt_match)
    {
        const vector<MapPoint *> vp_refKF_mpt = p_refKF->Get_vect_mappoints();
        vp_mpt_match = vector<MapPoint *>(cur_frame.mn_feature_num, static_cast<MapPoint *>(NULL));
        const DBoW3::FeatureVector &refKF_feature_vector = p_refKF->mv_featurevector;

        // cout<<refKF_feature_vector.size()<<endl;
        // cout<<cur_frame.mv_featurevector.size()<<endl;
        int nmatches = 0;
        const float bin_factor = mi_bin_len / 360.0f;
        vector<vector<int>> rot_hist(mi_bin_len);
        for (int i = 0; i < mi_bin_len; i++)
        {
            rot_hist[i].reserve(500);
        }

        int n_valueble_points = 0;
        for (auto p_mpt : vp_refKF_mpt)
        {
            if (!p_mpt)
            {
                continue;
            }
            if (p_mpt->Is_bad())
            {
                continue;
            }
            n_valueble_points++;
        }

        cout << "n_valueble_points:" << n_valueble_points << endl;

        DBoW3::FeatureVector::const_iterator refKF_it = refKF_feature_vector.begin();
        DBoW3::FeatureVector::const_iterator refKF_end = refKF_feature_vector.end();
        DBoW3::FeatureVector::const_iterator cur_frame_it = cur_frame.mv_featurevector.begin();
        DBoW3::FeatureVector::const_iterator cur_frame_end = cur_frame.mv_featurevector.end();

        int n_candidates = 0;
        int n_candidates2 = 0;
        // int n_it = 0;

        while (refKF_it != refKF_end && cur_frame_it != cur_frame_end)
        {
            if (refKF_it->first == cur_frame_it->first)
            {
                // cout << "n_it:" << n_it << endl;
                // n_it++;
                const vector<unsigned int> v_refKF_idx = refKF_it->second;
                const vector<unsigned int> v_cur_frame_idx = cur_frame_it->second;

                n_candidates2 = n_candidates2 + v_cur_frame_idx.size();
                for (int i = 0; i < v_refKF_idx.size(); i++)
                {
                    const unsigned int refKF_idx = v_refKF_idx[i];
                    MapPoint *p_mpt = vp_refKF_mpt[refKF_idx];
                    if (!p_mpt)
                    {
                        continue;
                    }
                    if (p_mpt->Is_bad())
                    {
                        continue;
                    }
                    n_candidates++;
                    const Mat &refKF_desc = p_refKF->mm_descriptors.row(refKF_idx);
                    int best_dist1 = 256;
                    int best_dist2 = 256;
                    int best_idx = -1;

                    for (int j = 0; j < v_cur_frame_idx.size(); j++)
                    {
                        const unsigned int cur_frame_idx = v_cur_frame_idx[j];
                        if (vp_mpt_match[cur_frame_idx])
                        {
                            continue;
                        }

                        // if(i==0){
                        //     n_candidates2++;
                        // }
                        const Mat &cur_frame_desc = cur_frame.mm_descriptors.row(cur_frame_idx);
                        const int dist = Compute_descriptor_distance(refKF_desc, cur_frame_desc);

                        if (dist < best_dist1)
                        {
                            best_dist2 = best_dist1;
                            best_dist1 = dist;
                            best_idx = cur_frame_idx;
                        }
                        else if (dist < best_dist2)
                        {
                            best_dist2 = dist;
                        }
                    }

                    if (best_dist1 <= mi_th_low)
                    {
                        if (static_cast<float>(best_dist1) < static_cast<float>(best_dist2) * mf_best_secondbest_ratio)
                        {
                            vp_mpt_match[best_idx] = p_mpt;
                            const KeyPoint &refKF_kp = p_refKF->mv_orb_unkeypoints[refKF_idx];
                            if (mb_check_orientation)
                            {
                                KeyPoint &cur_frame_kp = cur_frame.mv_orb_unkeypoints[best_idx];
                                float rot = refKF_kp.angle - cur_frame_kp.angle;
                                if (rot < 0.0)
                                {
                                    rot += 360.0f;
                                }
                                int bin = cvFloor(rot * bin_factor);
                                if (bin == mi_bin_len)
                                {
                                    bin = 0;
                                }
                                assert(bin >= 0 && bin < mi_bin_len);
                                rot_hist[bin].push_back(best_idx);
                            }
                            nmatches++;
                        }
                    }
                }
                refKF_it++;
                cur_frame_it++;
            }
            else if (refKF_it->first < cur_frame_it->first)
            {
                refKF_it = refKF_feature_vector.lower_bound(cur_frame_it->first);
            }
            else
            {
                cur_frame_it = cur_frame.mv_featurevector.lower_bound(refKF_it->first);
            }
        }
        cout << "n_candidates:" << n_candidates << endl;
        cout << "n_candidates2:" << n_candidates2 << endl;
        if (mb_check_orientation)
        {
            int idx1 = -1;
            int idx2 = -1;
            int idx3 = -1;
            Compute_three_maxima(rot_hist, mi_bin_len, idx1, idx2, idx3);
            for (int i = 0; i < mi_bin_len; i++)
            {
                if (i == idx1 || i == idx2 || i == idx3)
                {
                    continue;
                }
                for (int j = 0; j < rot_hist[i].size(); j++)
                {
                    vp_mpt_match[rot_hist[i][j]] = static_cast<MapPoint *>(NULL);
                    nmatches--;
                }
            }
        }
        return nmatches;
    }

    int Matcher::Search_by_bow(KeyFrame *p_keyframe1, KeyFrame *p_keyframe2, vector<MapPoint *> &vp_match12)
    {
        const vector<KeyPoint> &v_unkp1 = p_keyframe1->mv_orb_unkeypoints;
        const DBoW3::FeatureVector &v_feature_vector1 = p_keyframe1->mv_featurevector;
        const vector<MapPoint *> vp_mpt1 = p_keyframe1->Get_vect_mappoints();
        const Mat &desc1 = p_keyframe1->mm_descriptors;

        const vector<KeyPoint> &v_unkp2 = p_keyframe2->mv_orb_unkeypoints;
        const DBoW3::FeatureVector &v_feature_vector2 = p_keyframe2->mv_featurevector;
        const vector<MapPoint *> vp_mpt2 = p_keyframe2->Get_vect_mappoints();
        const Mat &desc2 = p_keyframe2->mm_descriptors;

        vp_match12 = vector<MapPoint *>(vp_mpt1.size(), static_cast<MapPoint *>(NULL));
        vector<bool> vb_matched2(vp_mpt2.size(), false);

        const float bin_factor = mi_bin_len / 360.0f;
        vector<vector<int>> rot_hist(mi_bin_len);
        for (int i = 0; i < mi_bin_len; i++)
        {
            rot_hist[i].reserve(500);
        }

        int n_match = 0;

        DBoW3::FeatureVector::const_iterator f1it = v_feature_vector1.begin();
        DBoW3::FeatureVector::const_iterator f2it = v_feature_vector2.begin();
        DBoW3::FeatureVector::const_iterator f1end = v_feature_vector1.end();
        DBoW3::FeatureVector::const_iterator f2end = v_feature_vector2.end();

        while (f1it != f1end && f2it != f2end)
        {
            if (f1it->first == f2it->first)
            {
                for (int i1 = 0; i1 < f1it->second.size(); i1++)
                {
                    const int idx1 = f1it->second[i1];
                    MapPoint *p_mpt1 = vp_mpt1[idx1];
                    if (!p_mpt1 || p_mpt1->Is_bad())
                    {
                        continue;
                    }
                    const Mat &mpt1_desc = desc1.row(idx1);
                    int best_dist1 = 256;
                    int best_idx2 = -1;
                    int best_dist2 = 256;

                    for (int i2 = 0; i2 < f2it->second.size(); i2++)
                    {
                        const int idx2 = f2it->second[i2];
                        MapPoint *p_mpt2 = vp_mpt2[idx2];
                        if (!p_mpt2 || p_mpt2->Is_bad())
                        {
                            continue;
                        }
                        if (vb_matched2[idx2])
                        {
                            continue;
                        }
                        const Mat &mpt2_desc = desc2.row(idx2);
                        int dist = Compute_descriptor_distance(mpt1_desc, mpt2_desc);

                        if (dist < best_dist1)
                        {
                            best_dist2 = best_dist1;
                            best_dist1 = dist;
                            best_idx2 = idx2;
                        }
                        else if (dist < best_dist2)
                        {
                            best_dist2 = dist;
                        }
                    }
                    if (best_dist1 < mi_th_low)
                    {
                        if (static_cast<float>(best_dist1) < mf_best_secondbest_ratio * static_cast<float>(best_dist2))
                        {
                            vp_match12[idx1] = vp_mpt2[best_idx2];
                            vb_matched2[best_idx2] = true;
                            n_match++;
                            if (mb_check_orientation)
                            {

                                float rot = v_unkp1[idx1].angle - v_unkp2[best_idx2].angle;
                                if (rot < 0.0)
                                {
                                    rot += 360.0f;
                                }
                                int bin = cvFloor(rot * bin_factor);
                                if (bin == mi_bin_len)
                                {
                                    bin = 0;
                                }
                                assert(bin >= 0 && bin < mi_bin_len);
                                rot_hist[bin].push_back(idx1);
                            }
                        }
                    }
                }
                f1it++;
                f2it++;
            }
            else if (f1it->first < f2it->first)
            {
                f1it = v_feature_vector1.lower_bound(f2it->first);
            }
            else
            {
                f2it = v_feature_vector2.lower_bound(f1it->first);
            }
        }
        if (mb_check_orientation)
        {
            int idx1 = -1;
            int idx2 = -1;
            int idx3 = -1;
            Compute_three_maxima(rot_hist, mi_bin_len, idx1, idx2, idx3);
            for (int i = 0; i < mi_bin_len; i++)
            {
                if (i == idx1 || i == idx2 || i == idx3)
                {
                    continue;
                }
                for (int j = 0; j < rot_hist[i].size(); j++)
                {
                    vp_match12[rot_hist[i][j]] = static_cast<MapPoint *>(NULL);
                    n_match--;
                }
            }
        }
        return n_match;
    }

    void Matcher::Compute_three_maxima(vector<vector<int>> &histo, int histo_len, int &idx1, int &idx2, int &idx3)
    {
        int max1 = 0;
        int max2 = 0;
        int max3 = 0;
        for (int i = 0; i < histo_len; i++)
        {
            const int s = histo[i].size();
            if (s > max1)
            {
                max3 = max2;
                max2 = max1;
                max1 = s;
                idx3 = idx2;
                idx2 = idx1;
                idx1 = i;
            }
            else if (s > max2)
            {
                max3 = max2;
                max2 = s;
                idx3 = idx2;
                idx2 = i;
            }
            else if (s > max3)
            {
                max3 = s;
                idx3 = i;
            }
        }
        if (max2 < 0.1f * (float)max1)
        {
            idx2 = -1;
            idx3 = -1;
        }
        else if (max3 < 0.1f * (float)max1)
        {
            idx3 = -1;
        }
    }

    void Matcher::Set_matcher_parameters(int th_low, int th_high, float best_second_ratio, bool check_orientation, int bin_len)
    {
        mi_th_low = th_low;
        mi_th_high = th_high;
        mf_best_secondbest_ratio = best_second_ratio;
        mb_check_orientation = check_orientation;
        mi_bin_len = bin_len;
    }

    int Matcher::Search_by_projection(Frame &last_frme, Frame &cur_frame, const float th)
    {
        int nmatches = 0;
        const float bin_factor = mi_bin_len / 360.0f;
        vector<vector<int>> rot_hist(mi_bin_len);
        for (int i = 0; i < mi_bin_len; i++)
        {
            rot_hist[i].reserve(500);
        }

        const Mat Rcw = cur_frame.Get_Rcw();
        const Mat tcw = cur_frame.Get_tcw();
        const Mat twc = cur_frame.Get_twc();

        const Mat Rlw = last_frme.Get_Rcw();
        const Mat tlw = last_frme.Get_tcw();
        const Mat twl = last_frme.Get_twc();

        const Mat tlc = Rlw * twc + tlw;

        // int num_pt = 0;

        for (int i = 0; i < last_frme.mn_feature_num; i++)
        {
            MapPoint *p_mpt = last_frme.mvp_mappoints[i];
            if (p_mpt)
            {
                if (!last_frme.mvb_outline[i])
                {
                    // num_pt++;
                    Mat p3dw = p_mpt->Get_world_pose();
                    Mat p3dc = Rcw * p3dw + tcw;
                    const float invzc = 1.0 / p3dc.at<float>(2);
                    if (invzc < 0)
                    {
                        continue;
                    }
                    Point2f uv = Converter::P3d_to_pointt2f(p3dc);
                    if (uv.x < cur_frame.mf_box_minx || uv.x > cur_frame.mf_box_maxx)
                    {
                        continue;
                    }
                    if (uv.y < cur_frame.mf_box_miny || uv.y > cur_frame.mf_box_maxy)
                    {
                        continue;
                    }
                    int octave = last_frme.mv_orb_unkeypoints[i].octave;
                    float radius = th * cur_frame.mvd_scale_factor[octave];
                    vector<int> v_idx2;

                    v_idx2 = cur_frame.Get_candidate_points_to_match(uv, radius, octave - 1, octave + 1);

                    if (v_idx2.empty())
                    {
                        continue;
                    }
                    const Mat mpt_desc = p_mpt->Get_descriptor();
                    int best_dist = 256;
                    int best_idx = -1;

                    for (vector<int>::const_iterator vit = v_idx2.begin(); vit != v_idx2.end(); vit++)
                    {
                        const int i2 = *vit;
                        if (cur_frame.mvp_mappoints[i2])
                        {
                            // if (cur_frame.mvp_mappoints[i2]->Get_observation_num() > 0)
                            // {
                            //     continue;
                            // }
                            best_dist = 256;
                            best_idx = -1;
                            break;
                        }
                        const Mat &candidate_desc = cur_frame.mm_descriptors.row(i2);
                        const int dist = Compute_descriptor_distance(mpt_desc, candidate_desc);
                        if (dist < best_dist)
                        {
                            best_dist = dist;
                            best_idx = i2;
                        }
                    }
                    if (best_dist <= mi_th_high)
                    {
                        cur_frame.mvp_mappoints[best_idx] = p_mpt;
                        nmatches++;
                        if (mb_check_orientation)
                        {
                            float rot = last_frme.mv_orb_unkeypoints[i].angle - cur_frame.mv_orb_unkeypoints[best_idx].angle;
                            if (rot < 0.0)
                            {
                                rot += 360.0f;
                            }
                            int bin = cvFloor(rot * bin_factor);
                            if (bin == mi_bin_len)
                            {
                                bin = 0;
                            }
                            assert(bin >= 0 && bin < mi_bin_len);
                            rot_hist[bin].push_back(best_idx);
                        }
                    }
                }
            }
        }
        // cout << "num_pt:" << num_pt << endl;
        if (mb_check_orientation)
        {
            int idx1;
            int idx2;
            int idx3;

            Compute_three_maxima(rot_hist, mi_bin_len, idx1, idx2, idx3);
            for (int i = 0; i < mi_bin_len; i++)
            {
                if (i != idx1 && i != idx2 && i != idx3)
                {
                    for (int j = 0; j < rot_hist[i].size(); j++)
                    {
                        cur_frame.mvp_mappoints[rot_hist[i][j]] = static_cast<MapPoint *>(NULL);
                        nmatches--;
                    }
                }
            }
        }
        return nmatches;
    }

    int Matcher::Search_by_projection(Frame &cur_frame, const vector<MapPoint *> &vp_local_mappoints, int search_radius, bool flag)
    {
        int tmp_ntomatch = 0;
        int nmatches = 0;
        for (auto p_mpt : vp_local_mappoints)
        {
            if (p_mpt->mb_ifproj_tocurframe)
            {
                tmp_ntomatch++;
                const int level = p_mpt->mn_tarcked_level;
                float r;
                if (p_mpt->mf_viewcos_tocurframe > 0.998)
                {
                    r = 2.5;
                }
                else
                {
                    r = 4;
                }
                if (flag)
                {
                    search_radius = r * search_radius;
                }

                const vector<int> v_idx = cur_frame.Get_candidate_points_to_match(p_mpt->m_preproj_uv_tocurframe, search_radius * cur_frame.mvd_scale_factor[level], level - 1, level);

                if (!v_idx.empty())
                {
                    const Mat mpt_desc = p_mpt->Get_descriptor();
                    int bestdist1 = 256;
                    int bestdist2 = 256;
                    int bestlevel1 = -1;
                    int bestlevel2 = -1;
                    int bestidx = -1;

                    for (auto idx : v_idx)
                    {
                        if (cur_frame.mvp_mappoints[idx])
                        {
                            if (cur_frame.mvp_mappoints[idx]->Get_observation_num() > 0)
                            {
                                continue;
                            }
                        }

                        const Mat fpt_desc = cur_frame.mm_descriptors.row(idx);

                        const int dist = Compute_descriptor_distance(mpt_desc, fpt_desc);

                        if (dist < bestdist1)
                        {
                            bestdist2 = bestdist1;
                            bestdist1 = dist;
                            bestlevel2 = bestlevel1;
                            bestlevel1 = cur_frame.mv_orb_unkeypoints[idx].octave;
                            bestidx = idx;
                        }
                        else if (dist < bestdist2)
                        {
                            bestlevel2 = cur_frame.mv_orb_unkeypoints[idx].octave;
                            bestdist2 = dist;
                        }
                    }
                    if (bestdist1 < mi_th_high)
                    {
                        if (bestlevel1 == bestlevel2 && bestdist1 > mf_best_secondbest_ratio * bestdist2)
                        {
                            continue;
                        }
                        if (bestlevel1 != bestlevel2 || bestdist1 < mf_best_secondbest_ratio * bestdist2)
                        {
                            cur_frame.mvp_mappoints[bestidx] = p_mpt;
                            nmatches++;
                        }
                    }
                }
            }
        }
        // cout<<"tmp_ntomatch:"<<tmp_ntomatch<<endl;
        return nmatches;
    }

    int Matcher::Search_by_projection(KeyFrame *p_keyframe, Mat mat_Scw, const vector<MapPoint *> &vp_mpt, const vector<KeyFrame *> &vp_mpt_keyframe, vector<MapPoint *> &vp_matched_mpt, vector<KeyFrame *> &vp_matched_keyframe, int th, float ratio_hamming)
    {
        Mat sRcw = mat_Scw.rowRange(0, 3).colRange(0, 3);
        const float scw = sqrt(sRcw.row(0).dot(sRcw.row(0)));
        Mat Rcw = sRcw / scw;
        Mat tcw = mat_Scw.rowRange(0, 3).col(3) / scw;
        Mat twc = -Rcw.t() * tcw;

        set<MapPoint *> sp_already_found(vp_matched_mpt.begin(), vp_matched_mpt.end());
        sp_already_found.erase(static_cast<MapPoint *>(NULL));

        int n_matches = 0;

        for (int i = 0; i < vp_mpt.size(); i++)
        {
            MapPoint *p_mpt = vp_mpt[i];
            KeyFrame *p_keyframe1 = vp_mpt_keyframe[i];
            if (p_mpt->Is_bad() || sp_already_found.count(p_mpt))
            {
                continue;
            }
            Mat p3dw = p_mpt->Get_world_pose();
            Mat p3dc = Rcw * p3dw + tcw;

            const float invzc = 1.0 / p3dc.at<float>(2);
            if (invzc < 0)
            {
                continue;
            }
            Point2f uv = Converter::P3d_to_pointt2f(p3dc);
            if (uv.x < p_keyframe->mf_box_minx || uv.x > p_keyframe->mf_box_maxx)
            {
                continue;
            }
            if (uv.y < p_keyframe->mf_box_miny || uv.y > p_keyframe->mf_box_maxy)
            {
                continue;
            }

            Mat po = p3dw - twc;
            float dist3d = norm(po);

            const float max_distance = p_mpt->Get_max_distance_invariance();
            const float min_distance = p_mpt->Get_min_distance_invariance();

            if (dist3d < min_distance || dist3d > max_distance)
            {
                continue;
            }

            Mat pn = p_mpt->Get_normal();

            if (po.dot(pn) < 0.5 * dist3d)
            {
                continue;
            }

            int n_predict_level = p_mpt->Predict_scale(dist3d, p_keyframe);

            const float radius = th * p_keyframe->mvd_scale_factor[n_predict_level];

            const vector<int> vn_indices = p_keyframe->Get_candidate_points_to_match(uv, radius, n_predict_level - 1, n_predict_level + 1);
            if (vn_indices.empty())
            {
                continue;
            }

            const Mat mpt_desc = p_mpt->Get_descriptor();
            int best_dist = 256;
            int best_idx = -1;

            for (vector<int>::const_iterator vit = vn_indices.begin(); vit != vn_indices.end(); vit++)
            {
                const int i2 = *vit;
                if (p_keyframe->mvp_mappoints[i2])
                {
                    best_dist = 256;
                    best_idx = -1;
                    break;
                }
                const Mat candidate_desc = p_keyframe->mm_descriptors.row(i2);
                const int dist = Compute_descriptor_distance(mpt_desc, candidate_desc);
                if (dist < best_dist)
                {
                    best_dist = dist;
                    best_idx = i2;
                }
            }

            if (best_dist <= mi_th_low * ratio_hamming)
            {
                vp_matched_mpt[best_idx] = p_mpt;
                vp_matched_keyframe[best_idx] = p_keyframe1;
                n_matches++;
            }
        }
        return n_matches;
    }

    int Matcher::Search_by_projection(KeyFrame *p_keyframe, Mat mat_Scw, const vector<MapPoint *> &vp_mpt, vector<MapPoint *> &vp_matched_mpt, int th, float ratio_hamming)
    {
        Mat sRcw = mat_Scw.rowRange(0, 3).colRange(0, 3);
        const float scw = sqrt(sRcw.row(0).dot(sRcw.row(0)));
        Mat Rcw = sRcw / scw;
        Mat tcw = mat_Scw.rowRange(0, 3).col(3) / scw;
        Mat twc = -Rcw.t() * tcw;

        set<MapPoint *> sp_already_found(vp_matched_mpt.begin(), vp_matched_mpt.end());
        sp_already_found.erase(static_cast<MapPoint *>(NULL));

        int n_matches = 0;

        for (int i = 0; i < vp_mpt.size(); i++)
        {
            MapPoint *p_mpt = vp_mpt[i];
            if (p_mpt->Is_bad() || sp_already_found.count(p_mpt))
            {
                continue;
            }
            Mat p3dw = p_mpt->Get_world_pose();
            Mat p3dc = Rcw * p3dw + tcw;

            const float invzc = 1.0 / p3dc.at<float>(2);
            if (invzc < 0)
            {
                continue;
            }
            Point2f uv = Converter::P3d_to_pointt2f(p3dc);
            if (uv.x < p_keyframe->mf_box_minx || uv.x > p_keyframe->mf_box_maxx)
            {
                continue;
            }
            if (uv.y < p_keyframe->mf_box_miny || uv.y > p_keyframe->mf_box_maxy)
            {
                continue;
            }

            Mat po = p3dw - twc;
            float dist3d = norm(po);

            const float max_distance = p_mpt->Get_max_distance_invariance();
            const float min_distance = p_mpt->Get_min_distance_invariance();

            if (dist3d < min_distance || dist3d > max_distance)
            {
                continue;
            }

            Mat pn = p_mpt->Get_normal();

            if (po.dot(pn) < 0.5 * dist3d)
            {
                continue;
            }

            int n_predict_level = p_mpt->Predict_scale(dist3d, p_keyframe);

            const float radius = th * p_keyframe->mvd_scale_factor[n_predict_level];

            const vector<int> vn_indices = p_keyframe->Get_candidate_points_to_match(uv, radius, n_predict_level - 1, n_predict_level + 1);
            if (vn_indices.empty())
            {
                continue;
            }

            const Mat mpt_desc = p_mpt->Get_descriptor();
            int best_dist = 256;
            int best_idx = -1;

            for (vector<int>::const_iterator vit = vn_indices.begin(); vit != vn_indices.end(); vit++)
            {
                const int i2 = *vit;
                if (p_keyframe->mvp_mappoints[i2])
                {
                    best_dist = 256;
                    best_idx = -1;
                    break;
                }
                const Mat candidate_desc = p_keyframe->mm_descriptors.row(i2);
                const int dist = Compute_descriptor_distance(mpt_desc, candidate_desc);
                if (dist < best_dist)
                {
                    best_dist = dist;
                    best_idx = i2;
                }
            }

            if (best_dist <= mi_th_low * ratio_hamming)
            {
                vp_matched_mpt[best_idx] = p_mpt;
                n_matches++;
            }
        }
        return n_matches;
    }

    int Matcher::Search_by_projection_relocalization(Frame &cur_frame, KeyFrame *p_keyframe, const set<MapPoint *> &sp_already_found, float th, int orb_dist)
    {
        int nmatches = 0;

        const Mat Rcw = cur_frame.Get_Rcw();
        const Mat tcw = cur_frame.Get_tcw();
        const Mat twc = cur_frame.Get_twc();

        const float bin_factor = mi_bin_len / 360.0f;
        vector<vector<int>> rot_hist(mi_bin_len);
        for (int i = 0; i < mi_bin_len; i++)
        {
            rot_hist[i].reserve(500);
        }

        const vector<MapPoint *> vp_mpt = p_keyframe->Get_vect_mappoints();
        for (int i = 0; i < vp_mpt.size(); i++)
        {
            MapPoint *p_mpt = vp_mpt[i];
            if (p_mpt)
            {
                if (!p_mpt->Is_bad() && !sp_already_found.count(p_mpt))
                {
                    Mat p3dw = p_mpt->Get_world_pose();
                    Mat p3dc = Rcw * p3dw + tcw;

                    const float invzc = 1.0 / p3dc.at<float>(2);
                    if (invzc < 0)
                    {
                        continue;
                    }
                    Point2f uv = Converter::P3d_to_pointt2f(p3dc);
                    if (uv.x < cur_frame.mf_box_minx || uv.x > cur_frame.mf_box_maxx)
                    {
                        continue;
                    }
                    if (uv.y < cur_frame.mf_box_miny || uv.y > cur_frame.mf_box_maxy)
                    {
                        continue;
                    }

                    Mat po = p3dw - twc;
                    float dist3d = norm(po);

                    const float max_distance = p_mpt->Get_max_distance_invariance();
                    const float min_distance = p_mpt->Get_min_distance_invariance();

                    if (dist3d < min_distance || dist3d > max_distance)
                    {
                        continue;
                    }

                    int n_predict_level = p_mpt->Predict_scale(dist3d, &cur_frame);

                    const float radius = th * cur_frame.mvd_scale_factor[n_predict_level];

                    vector<int> v_idx2 = cur_frame.Get_candidate_points_to_match(uv, radius, n_predict_level - 1, n_predict_level + 1);

                    if (v_idx2.empty())
                    {
                        continue;
                    }

                    const Mat mpt_desc = p_mpt->Get_descriptor();

                    int best_dist = 256;
                    int best_idx = -1;

                    for (vector<int>::const_iterator vit = v_idx2.begin(); vit != v_idx2.end(); vit++)
                    {
                        const int i2 = *vit;
                        if (cur_frame.mvp_mappoints[i2])
                        {
                            // if (cur_frame.mvp_mappoints[i2]->Get_observation_num() > 0)
                            // {
                            //     continue;
                            // }
                            best_dist = 256;
                            best_idx = -1;
                            break;
                        }
                        const Mat &candidate_desc = cur_frame.mm_descriptors.row(i2);
                        const int dist = Compute_descriptor_distance(mpt_desc, candidate_desc);
                        if (dist < best_dist)
                        {
                            best_dist = dist;
                            best_idx = i2;
                        }
                    }
                    if (best_dist <= orb_dist)
                    {
                        cur_frame.mvp_mappoints[best_idx] = p_mpt;
                        nmatches++;
                        if (mb_check_orientation)
                        {
                            float rot = p_keyframe->mv_orb_unkeypoints[i].angle - cur_frame.mv_orb_unkeypoints[best_idx].angle;
                            if (rot < 0.0)
                            {
                                rot += 360.0f;
                            }
                            int bin = cvFloor(rot * bin_factor);
                            if (bin == mi_bin_len)
                            {
                                bin = 0;
                            }
                            assert(bin >= 0 && bin < mi_bin_len);
                            rot_hist[bin].push_back(best_idx);
                        }
                    }
                }
            }
        }

        if (mb_check_orientation)
        {
            int idx1;
            int idx2;
            int idx3;

            Compute_three_maxima(rot_hist, mi_bin_len, idx1, idx2, idx3);
            for (int i = 0; i < mi_bin_len; i++)
            {
                if (i != idx1 && i != idx2 && i != idx3)
                {
                    for (int j = 0; j < rot_hist[i].size(); j++)
                    {
                        cur_frame.mvp_mappoints[rot_hist[i][j]] = static_cast<MapPoint *>(NULL);
                        nmatches--;
                    }
                }
            }
        }
        return nmatches;
    }

    int Matcher::Search_for_triangulation(KeyFrame *p_keyframe1, KeyFrame *p_keyframe2, Matx33f F12, vector<pair<int, int>> &v_match_idx, const bool b_only_stereo, const bool b_coarse)
    {
        const DBoW3::FeatureVector &v_feature_vector1 = p_keyframe1->mv_featurevector;
        const DBoW3::FeatureVector &v_feature_vector2 = p_keyframe2->mv_featurevector;

        auto t1wc = p_keyframe1->Get_twc_();
        auto R2w = p_keyframe2->Get_Rcw_();
        auto t2w = p_keyframe2->Get_tcw_();

        auto t21 = R2w * t1wc + t2w;

        Point2f ep = Converter::P3d_to_pointt2f(Point3f(t21(0), t21(1), t21(2)));

        int nmatches = 0;
        vector<bool> vb_matched2(p_keyframe2->mn_feature_num, false);
        vector<int> v_matches12(p_keyframe1->mn_feature_num, -1);

        vector<vector<int>> rot_hist(mi_bin_len);

        for (int i = 0; i < mi_bin_len; i++)
        {
            rot_hist[i].reserve(500);
        }

        const float bin_factor = 1.0f / mi_bin_len;

        DBoW3::FeatureVector::const_iterator f1it = v_feature_vector1.begin();
        DBoW3::FeatureVector::const_iterator f2it = v_feature_vector2.begin();
        DBoW3::FeatureVector::const_iterator f1end = v_feature_vector1.end();
        DBoW3::FeatureVector::const_iterator f2end = v_feature_vector2.end();

        while (f1it != f1end && f2it != f2end)
        {
            if (f1it->first == f2it->first)
            {
                for (int i1 = 0; i1 < f1it->second.size(); i1++)
                {
                    const int idx1 = f1it->second[i1];
                    MapPoint *p_mpt1 = p_keyframe1->Get_mappoint(idx1);

                    if (p_mpt1)
                    {
                        continue;
                    }

                    const KeyPoint &kp1 = p_keyframe1->mv_orb_unkeypoints[idx1];

                    const Mat &desc1 = p_keyframe1->mm_descriptors.row(idx1);

                    int best_dist = mi_th_low;

                    int best_idx2 = -1;

                    for (int i2 = 0; i2 < f2it->second.size(); i2++)
                    {

                        int idx2 = f2it->second[i2];
                        MapPoint *p_mpt2 = p_keyframe2->Get_mappoint(idx2);

                        if (vb_matched2[idx2] || p_mpt2)
                        {
                            continue;
                        }

                        const Mat &desc2 = p_keyframe2->mm_descriptors.row(idx2);

                        const int dist = Compute_descriptor_distance(desc1, desc2);

                        if (dist > mi_th_low || dist > best_dist)
                        {
                            continue;
                        }

                        const KeyPoint &kp2 = p_keyframe2->mv_orb_unkeypoints[idx2];

                        const float dist_ex = ep.x - kp2.pt.x;
                        const float dist_ey = ep.y - kp2.pt.y;
                        if (dist_ex * dist_ex + dist_ey * dist_ey < 100 * p_keyframe2->mvd_scale_factor[kp2.octave])
                        {
                            continue;
                        }

                        if (Epipolar_constrain(kp1, kp2, F12, p_keyframe1->mvd_level_sigma2[kp1.octave], p_keyframe2->mvd_level_sigma2[kp2.octave]) || b_coarse)
                        {
                            best_idx2 = idx2;
                            best_dist = dist;
                        }
                    }
                    if (best_idx2 > 0)
                    {
                        KeyPoint &kp2 = p_keyframe2->mv_orb_unkeypoints[best_idx2];
                        v_matches12[idx1] = best_idx2;
                        vb_matched2[best_idx2] = true;
                        nmatches++;

                        if (mb_check_orientation)
                        {
                            float rot = kp1.angle - kp2.angle;
                            if (rot < 0.0)
                            {
                                rot += 360.0f;
                            }
                            int bin = cvFloor(rot * bin_factor);
                            if (bin == mi_bin_len)
                            {
                                bin = 0;
                            }
                            assert(bin >= 0 && bin < mi_bin_len);
                            rot_hist[bin].push_back(idx1);
                        }
                    }
                }

                f1it++;
                f2it++;
            }
            else if (f1it->first < f2it->first)
            {
                f1it = v_feature_vector1.lower_bound(f2it->first);
            }
            else
            {
                f2it = v_feature_vector2.lower_bound(f1it->first);
            }
        }
        if (mb_check_orientation)
        {
            int idx1 = -1;
            int idx2 = -1;
            int idx3 = -1;
            Compute_three_maxima(rot_hist, mi_bin_len, idx1, idx2, idx3);
            for (int i = 0; i < mi_bin_len; i++)
            {
                if (i == idx1 || i == idx2 || i == idx3)
                {
                    continue;
                }
                for (int j = 0; j < rot_hist[i].size(); j++)
                {
                    v_matches12[rot_hist[i][j]] = -1;
                    nmatches--;
                }
            }
        }
        v_match_idx.clear();
        v_match_idx.reserve(nmatches);
        for (int i = 0; i < v_matches12.size(); i++)
        {
            if (v_matches12[i] < 0)
            {
                continue;
            }
            v_match_idx.push_back(make_pair(i, v_matches12[i]));
        }
        return nmatches;
    }

    bool Matcher::Epipolar_constrain(const KeyPoint &kp1, const KeyPoint &kp2, Matx33f &F12, const float sigmalevel, const float unc)
    {
        const float a = kp1.pt.x * F12(0, 0) + kp1.pt.y * F12(1, 0) + F12(2, 0);
        const float b = kp1.pt.x * F12(0, 1) + kp1.pt.y * F12(1, 1) + F12(2, 1);
        const float c = kp1.pt.x * F12(0, 2) + kp1.pt.y * F12(1, 2) + F12(2, 2);

        const float num = a * kp2.pt.x + b * kp2.pt.y + c;
        const float den = a * a + b * b;
        if (den == 0)
        {
            return false;
        }
        const float dspr = num * num / den;

        return dspr < 3.84 * unc;
    }

    int Matcher::Fuse(KeyFrame *p_keyframe, const vector<MapPoint *> &vp_mpt, const float th)
    {
        Mat Rcw, tcw, twc;

        Rcw = p_keyframe->Get_Rcw();
        tcw = p_keyframe->Get_tcw();
        twc = p_keyframe->Get_twc();

        int n_fused = 0;

        const int n_mpt = vp_mpt.size();

        for (int i = 0; i < n_mpt; i++)
        {
            MapPoint *p_mpt = vp_mpt[i];
            if (!p_mpt)
            {
                continue;
            }
            if (p_mpt->Is_bad())
            {
                continue;
            }
            else if (p_mpt->Is_in_keyframe(p_keyframe))
            {
                continue;
            }

            Mat p3dw = p_mpt->Get_world_pose();
            Mat p3dc = Rcw * p3dw + tcw;
            if (p3dc.at<float>(2) < 0.0f)
            {
                continue;
            }

            const float invz = 1 / p3dc.at<float>(2);
            const float x = p3dc.at<float>(0);
            const float y = p3dc.at<float>(1);
            const float z = p3dc.at<float>(2);

            const Point2f uv = Converter::P3d_to_pointt2f(Point3f(x, y, z));

            if (!p_keyframe->Is_in_image(uv.x, uv.y))
            {
                continue;
            }

            const float max_distance = p_mpt->Get_max_distance_invariance();
            const float min_distance = p_mpt->Get_min_distance_invariance();

            Mat po = p3dw - twc;

            const float distance = norm(po);
            if (distance < min_distance || distance > max_distance)
            {
                continue;
            }

            Mat pn = p_mpt->Get_normal();
            if (po.dot(pn) < 0.5 * distance)
            {
                continue;
            }

            int n_predicted_level = p_mpt->Predict_scale(distance, p_keyframe);

            const float radius = th * p_keyframe->mvd_scale_factor[n_predicted_level];

            const vector<int> v_indices = p_keyframe->Get_candidate_points_to_match(uv, radius, n_predicted_level - 1, n_predicted_level);

            if (v_indices.empty())
            {
                continue;
            }

            const Mat desc1 = p_mpt->Get_descriptor();
            int best_dist = 256;
            int best_idx = -1;
            for (auto i : v_indices)
            {
                const KeyPoint &kp = p_keyframe->mv_orb_unkeypoints[i];

                const int &kp_level = kp.octave;
                const float &kpx = kp.pt.x;
                const float &kpy = kp.pt.y;
                const float ex = uv.x - kpx;
                const float ey = uv.y - kpy;
                const float e2 = ex * ex + ey * ey;

                if (e2 * p_keyframe->mvd_inv_level_sigma2[kp_level] > 5.99)
                {
                    continue;
                }

                const Mat desc2 = p_keyframe->mm_descriptors.row(i);
                const int dist = Compute_descriptor_distance(desc1, desc2);

                if (dist < best_dist)
                {
                    best_dist = dist;
                    best_idx = i;
                }
            }
            if (best_dist <= mi_th_low)
            {
                MapPoint *p_mpt_keyframe = p_keyframe->Get_mappoint(best_idx);
                if (p_mpt_keyframe)
                {
                    if (!p_mpt_keyframe->Is_bad())
                    {
                        if (p_mpt_keyframe->Get_observation_num() > p_mpt->Get_observation_num())
                        {
                            p_mpt->Replace(p_mpt_keyframe);
                        }
                        else if (p_mpt_keyframe->Get_observation_num() < p_mpt->Get_observation_num())
                        {
                            p_mpt_keyframe->Replace(p_mpt);
                        }
                        else
                        {
                            if (p_mpt_keyframe->mn_first_keyframe_id > p_mpt->mn_first_keyframe_id)
                            {
                                p_mpt_keyframe->Replace(p_mpt);
                            }
                            else
                            {
                                p_mpt->Replace(p_mpt_keyframe);
                            }
                        }
                    }
                }
                else
                {
                    p_mpt->Add_observation(p_keyframe, best_idx);
                    p_keyframe->Add_mappoint(p_mpt, best_idx);
                    p_mpt->Compute_distinctive_descriptors();
                    p_mpt->Update_normal_and_depth();
                    p_keyframe->Update_connections();
                }
                n_fused++;
            }
        }
        return n_fused;
    }
    
    int Matcher::Fuse(KeyFrame* p_keyframe,Mat mat_Scw,const vector<MapPoint*> &vp_mpt,float th,vector<MapPoint*> &vp_replace_mpt)
    {
        Mat mat_sRcw = mat_Scw.rowRange(0,3).colRange(0,3);
        const float scw = sqrt(mat_sRcw.row(0).dot(mat_sRcw.row(0)));
        Mat mat_Rcw = mat_sRcw/scw;
        Mat mat_tcw = mat_Scw.rowRange(0,3).col(3)/scw;
        Mat mat_twc = -mat_Rcw.t()*mat_tcw;

        const set<MapPoint*> sp_already_found = p_keyframe->Get_set_mappoints();
        int n_fused = 0;
        for(int i=0;i<vp_mpt.size();i++){
            MapPoint* p_mpt = vp_mpt[i];
            if(p_mpt->Is_bad()||sp_already_found.count(p_mpt)){
                continue;
            }
            Mat mat_p3dw = p_mpt->Get_world_pose();
            Mat mat_p3dc = mat_Rcw*mat_p3dw+mat_tcw;
            if(mat_p3dc.at<float>(2)<0.0f){
                continue;
            }
            const float x = mat_p3dc.at<float>(0);
            const float y = mat_p3dc.at<float>(1);
            const float z = mat_p3dc.at<float>(2);

            const Point2f uv = Converter::P3d_to_pointt2f(Point3f(x,y,z));
            if(!p_keyframe->Is_in_image(uv.x,uv.y)){
                continue;
            }
            const float max_distance = p_mpt->Get_max_distance_invariance();
            const float min_distance = p_mpt->Get_min_distance_invariance();
            Mat mat_po = mat_p3dw-mat_twc;
            const float dist3d = norm(mat_po);

            if(dist3d<min_distance||dist3d>max_distance){
                continue;
            }
            Mat mat_pn = p_mpt->Get_normal();

            if(mat_po.dot(mat_pn)<0.5*dist3d){
                continue;
            }

            const int n_predicted_level = p_mpt->Predict_scale(dist3d,p_keyframe);

            const float radius = th*p_keyframe->mvd_scale_factor[n_predicted_level];
            const vector<int> vn_indices = p_keyframe->Get_candidate_points_to_match(uv,radius,n_predicted_level-1,n_predicted_level+1);
            if(vn_indices.empty()){
                continue;
            }
            const Mat mpt_desc = p_mpt->Get_descriptor();
            int best_dist = 256;
            int best_idx = -1;
            for(int j=0;j<vn_indices.size();j++){
                const int idx = vn_indices[j];
                const Mat match_desc = p_keyframe->mm_descriptors.row(idx);
                int dist = Compute_descriptor_distance(mpt_desc,match_desc);
                if(dist<best_dist){
                    best_dist = dist;
                    best_idx = idx;
                }
            }
            if(best_dist<=mi_th_low){
                MapPoint *p_mpt_in_keyframe = p_keyframe->Get_mappoint(best_idx);
                if(p_mpt_in_keyframe){
                    if(!p_mpt_in_keyframe->Is_bad()){
                        vp_replace_mpt[i] = p_mpt_in_keyframe;
                    }
                }else{
                    p_mpt->Add_observation(p_keyframe,best_idx);
                    p_keyframe->Add_mappoint(p_mpt,best_idx);

                }
                n_fused++;
            }
        }
        return n_fused;
    }
}